package org.noote.libs.spidy.anim;

import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;

import org.noote.libs.robot.Robot_Skeleton;
import org.noote.libs.robot.Robot_ServoMotor;
import org.noote.libs.sogle.Vector3D;
import org.noote.libs.spidy.comm.BluetoothCommandProtocol;

public class Spidy_BaseSkeleton extends Robot_Skeleton {
	
	protected final Lock lockUpdate = new ReentrantLock();

	public static final int LEFT_FRONT_COXA = 0, LEFT_FRONT_FEMUR = 1, LEFT_FRONT_TIBIA = 2;
	public static final int LEFT_MIDDLE_COXA = 3, LEFT_MIDDLE_FEMUR = 4, LEFT_MIDDLE_TIBIA = 5;
	public static final int LEFT_BACK_COXA = 6, LEFT_BACK_FEMUR = 7, LEFT_BACK_TIBIA = 8;
	public static final int RIGHT_FRONT_COXA = 9, RIGHT_FRONT_FEMUR = 10, RIGHT_FRONT_TIBIA = 11;
	public static final int RIGHT_MIDDLE_COXA = 12, RIGHT_MIDDLE_FEMUR = 13, RIGHT_MIDDLE_TIBIA = 14;
	public static final int RIGHT_BACK_COXA = 15, RIGHT_BACK_FEMUR = 16, RIGHT_BACK_TIBIA = 17;
	
	public Spidy_BaseSkeleton() {
		super();
	}
	
	public boolean create()
	{
		Robot_ServoMotor servo;
		
		//////////////////////////////////////////
		// create servos and associate to bones
		servo = newServo(LEFT_FRONT_COXA, 0, 90, 180, 0.21f/60.0f, false);
		servo.setAxis(90, new Vector3D(0, 1, 0));
		
		servo = newServo(LEFT_FRONT_FEMUR, 0, 90, 180, 0.21f/60.0f, false);
		servo.setAxis(-90, new Vector3D(0, 0, -1));

		servo = newServo(LEFT_FRONT_TIBIA, 0, 90, 180, 0.21f/60.0f, false);
		servo.setAxis(0, new Vector3D(0, 1, 0));

		servo = newServo(RIGHT_FRONT_COXA, 0, 90, 180, 0.21f/60.0f, false);
		servo.setAxis(0, new Vector3D(0, 1, 0));
		
		servo = newServo(RIGHT_FRONT_FEMUR, 0, 90, 180, 0.21f/60.0f, false);
		servo.setAxis(0, new Vector3D(0, 1, 0));

		servo = newServo(RIGHT_FRONT_TIBIA, 0, 90, 180, 0.21f/60.0f, false);
		servo.setAxis(0, new Vector3D(0, 1, 0));
		
		servo = newServo(LEFT_MIDDLE_COXA, 0, 90, 180, 0.21f/60.0f, false);
		servo.setAxis(0, new Vector3D(0, 1, 0));
		
		servo = newServo(LEFT_MIDDLE_FEMUR, 0, 90, 180, 0.21f/60.0f, false);
		servo.setAxis(0, new Vector3D(0, 1, 0));

		servo = newServo(LEFT_MIDDLE_TIBIA, 0, 90, 180, 0.21f/60.0f, false);
		servo.setAxis(0, new Vector3D(0, 1, 0));

		servo = newServo(RIGHT_MIDDLE_COXA, 0, 90, 180, 0.21f/60.0f, false);
		servo.setAxis(0, new Vector3D(0, 1, 0));
		
		servo = newServo(RIGHT_MIDDLE_FEMUR, 0, 90, 180, 0.21f/60.0f, false);
		servo.setAxis(0, new Vector3D(0, 1, 0));

		servo = newServo(RIGHT_MIDDLE_TIBIA, 0, 90, 180, 0.21f/60.0f, false);
		servo.setAxis(0, new Vector3D(0, 1, 0));
		
		servo = newServo(LEFT_BACK_COXA, 0, 90, 180, 0.21f/60.0f, false);
		servo.setAxis(0, new Vector3D(0, 1, 0));
		
		servo = newServo(LEFT_BACK_FEMUR, 0, 90, 180, 0.21f/60.0f, true);
		servo.setAxis(0, new Vector3D(0, 1, 0));

		servo = newServo(LEFT_BACK_TIBIA, 0, 90, 180, 0.21f/60.0f, true);
		servo.setAxis(0, new Vector3D(0, 1, 0));

		servo = newServo(RIGHT_BACK_COXA, 0, 90, 180, 0.21f/60.0f, false);
		servo.setAxis(0, new Vector3D(0, 1, 0));
		
		servo = newServo(RIGHT_BACK_FEMUR, 0, 90, 180, 0.21f/60.0f, true);
		servo.setAxis(0, new Vector3D(0, 1, 0));

		servo = newServo(RIGHT_BACK_TIBIA, 0, 90, 180, 0.21f/60.0f, true);
		servo.setAxis(0, new Vector3D(0, 1, 0));
		
		return true;
	}
	
	@Override
	public void update()
	{
		if(lockUpdate.tryLock())
		{
			try
			{
				super.update();
			}
			finally
			{
				lockUpdate.unlock();
			}
		}
	}
	
	public String buildTransmitMessage()
	{
		int nServos = 0;
		int servos_id[] = new int[getServoList().size()];
		float servos_pos[] = new float[servos_id.length];
		
		if(lockUpdate.tryLock())
		{
			try
			{
				for(Robot_ServoMotor s : getServoList())
				{
					if(s.isTransmittedHaveChangedPosition())
					{
						servos_id[nServos] = s.getID();
						servos_pos[nServos] = s.getRealPosition();
						s.flagTransmitted();
						nServos++;
					}
				}
			}
			finally
			{
				lockUpdate.unlock();
			}
		}
		
		if(nServos>0)
			return BluetoothCommandProtocol.sendSetServoPositions(0, nServos, servos_id, servos_pos);
		
		return "";
	}
}
